/*
 * manager.cpp
 *
 *  Created on: 6 mai 2012
 *      Author: user
 */
#include <signal.h>
#include <vector>

#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <image_transport/subscriber.h>
#include <image_transport/image_transport.h>

#include <cv_bridge/CvBridge.h>
#include <opencv2/highgui/highgui.hpp>
//#include "window_thread.h"

#include <boost/thread.hpp>
#include <boost/format.hpp>

#ifdef HAVE_GTK
#include <gtk/gtk.h>

// Platform-specific workaround for #3026: image_view doesn't close when
// closing image window. On platforms using GTK+ we connect this to the
// window's "destroy" event so that image_view exits.
static void destroyNode(GtkWidget *widget, gpointer data)
{
  /// @todo On ros::shutdown(), the node hangs. Why?
  //ros::shutdown();
  exit(0); // brute force solution
}

static void destroyNodelet(GtkWidget *widget, gpointer data)
{
  // We can't actually unload the nodelet from here, but we can at least
  // unsubscribe from the image topic.
  reinterpret_cast<image_transport::Subscriber*>(data)->shutdown();
}
#endif

void imageCb(const sensor_msgs::ImageConstPtr& msg);

static const char WINDOW_NAME[] = "Kinect View";

image_transport::Subscriber sub_;
sensor_msgs::CvBridge img_bridge_;

boost::mutex image_mutex_;
sensor_msgs::ImageConstPtr last_msg_;
IplImage* last_image_;

std::string window_name_= WINDOW_NAME;
boost::format filename_format_;
int count_;

//Vector to store images
std::vector<IplImage*> images_;
int width_ = 640;
int height_ = 480;

//Video writer pour enregistrer la video sur le disque
//
CvVideoWriter* writer = NULL;

// Signal-safe flag for whether shutdown is requested
sig_atomic_t volatile g_request_shutdown = 0;

// Replacement SIGINT handler
void mySigIntHandler(int sig)
{  
	g_request_shutdown = 1;
}

int main(int argc, char **argv)
{

	// Override SIGINT handler  
	ros::init(argc, argv, "manager", ros::init_options::NoSigintHandler);  signal(SIGINT, mySigIntHandler);
  
	ROS_WARN("##Je demarre");

  //ros::NodeHandle nh =ros::this_node;
  ros::NodeHandle n;
  ros::NodeHandle nh("~");

  // Command line argument parsing
  // First positional argument is the transport type
  std::string transport = "raw";
  for (int i = 0; i < argc; ++i)
  {
    if (argv[i][0] != '-')
    {
      transport = argv[i];
      break;
    }
  }

  cv::namedWindow(window_name_);

#ifdef HAVE_GTK
  // Register appropriate handler for when user closes the display window
  GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
  if (shutdown_on_close)
    g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
  else
    g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
#endif

  // Start the OpenCV window thread so we don't have to waitKey() somewhere
  //startWindowThread();

  image_transport::ImageTransport it(nh);
  sub_ = it.subscribe("/image", 3, &imageCb);



	//60 = fps // trouver un moyen de recuperer le fps de la kinect par le msg
	int fps = 5;	
	ROS_WARN("W : %d",width_);
	ROS_WARN("H : %d", height_);
	writer = cvCreateVideoWriter("out.avi", 0, fps, cvSize((int)width_,(int)height_), 1);
	ROS_WARN("-> cvCreateVideoWriter --");


	if (writer == NULL)
	{
		ROS_WARN("!!! ERROR: cvCreateVideoWriter\n");
		return -1;
	}

  	ROS_WARN("####spin");
  
	while (!g_request_shutdown)  
	{    
	// Do non-callback stuff    
	ros::spinOnce();     
	}

  	ROS_WARN("-> fin du spin");


	//Release du video writer
	//
	cvReleaseVideoWriter(&writer);
	ROS_WARN("-> cvReleaseVideoWriter");

	ros::shutdown();

  return 0;
}


//////////////////////////////////////////////////////////////////////////


void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
  ROS_WARN("####Encoding:%s",msg->encoding.c_str());
  image_mutex_.lock();
	

	//Set video width an height
	//
	width_ = msg->width;
	height_ = msg->height;


  // May want to view raw bayer data, which CvBridge doesn't know about
  if (msg->encoding.find("bayer") != std::string::npos)
  {
    cv::Mat mat_last_image_ = cv::Mat(msg->height, msg->width, CV_8UC1,
                          const_cast<uint8_t*>(&msg->data[0]), msg->step);

	IplImage tempImage = mat_last_image_;
	last_image_ = &tempImage;
  }
  // We want to scale floating point images so that they display nicely
  else if(msg->encoding.find("F") != std::string::npos)
  {

	  ROS_WARN("####Floating:");

	    cv::Mat float_image_bridge = img_bridge_.imgMsgToCv(msg, "32FC1");
	    cv::Mat_<float> float_image = float_image_bridge;
	    float max_val = 0;
	    for(int i = 0; i < float_image.rows; ++i)
	    {
	      for(int j = 0; j < float_image.cols; ++j)
	      {
	        max_val = std::max(max_val, float_image(i, j));
	      }
	    }

	    if(max_val > 0)
	    {
	      float_image /= max_val;
	    }
		IplImage tempImage = float_image;
	    last_image_ = &tempImage;
  }
  else
  {
    // Convert to OpenCV native BGR color
    try {
	
	IplImage* temp_image = img_bridge_.imgMsgToCv(msg, "bgr8");	
	cvWriteFrame(writer, temp_image);
	last_image_ = temp_image;	
	
    }
    catch (sensor_msgs::CvBridgeException& e) {
    	ROS_ERROR("Unable to convert '%s' image to bgr8",
                             msg->encoding.c_str());

    }
  }

  // last_image_ may point to data owned by last_msg_, so we hang onto it for
  // the sake of mouseCb.
  last_msg_ = msg;

  // Must release the mutex before calling cv::imshow, or can deadlock against
  // OpenCV's window mutex.
  image_mutex_.unlock();
  if (last_image_){
    cv::imshow(window_name_,  cv::Mat(last_image_,true));
    cv::waitKey(1);
  }
}



